import org.jbox2d.util.nonconvex.*;
import org.jbox2d.dynamics.contacts.*;
import org.jbox2d.testbed.*;
import org.jbox2d.collision.*;
import org.jbox2d.common.*;
import org.jbox2d.dynamics.joints.*;
import org.jbox2d.p5.*;
import org.jbox2d.dynamics.*;


import processing.opengl.*;

ClientOSC client;
int ch = 0;

// Number of links in chain
int numLinks = 10;

// Physics things we must store
Physics physics;
Body body, body2;
World world;


ArrayList boxes;

void setup() {
  size(1024,768,OPENGL);
  smooth();
  hint(ENABLE_OPENGL_4X_SMOOTH);
  frameRate(60);
  client = new ClientOSC();
  boxes = new ArrayList();
  initScene();
}


void initScene() {
  physics = new Physics(this, width, height);
  physics.setDensity(1.0f);
  physics.setFriction(0.0f);
  physics.setRestitution(0.9f);
  
  world = physics.getWorld();
  world.setGravity( new Vec2(0, 0) );

  world.setContactListener(new ListenContact());
  physics.setCustomRenderingMethod(this, "render");
}


void DumpContactPoint(ContactPoint cp)
{
  println("ContactPoint: " + cp + " (" + cp.id.features.incidentEdge + ")");
  println("- Shapes: " + cp.shape1 + ", " + cp.shape2);
  println("- Position: " + cp.position);
  println("- Velocity: " + cp.velocity);
  println("- Normal: " + cp.normal);
  println("- Separation: " + cp.separation);
  println("- Friction / Restitution: " + cp.friction + " / " + cp.restitution);
  
  Shape s1 = cp.shape1;
  Body b1 = s1.getBody();
  Object o1 = b1.getUserData();
  Box p = (Box) o1;
  p.hit();
  

  //cp.shape1
  // send message
}


void DumpContactResult(ContactResult cr)
{
  println("ContactResult: " + cr + " (" + cr.id.features.incidentEdge + ")");
  println("- Shapes: " + cr.shape1 + ", " + cr.shape2);
  println("- Position: " + cr.position);
  println("- Normal: " + cr.normal + " -> " + cr.normalImpulse + " / " + cr.tangentImpulse);
}


void mousePressed()
{
    addBox();
}


void keyPressed()
{
  if(key == 'a')
  {
    physics.destroy();
    initScene();      
  }
  
  if(key == ' ')
  {
     addBox();
  }
 
}


void addBox()
{
    Box bx = new Box(width*.5, height*.5);
    boxes.add(bx); 
    bx.id = int(random(0,20));//ch;
    ch++;
    println(ch);
}

// required empty
void draw() 
{
}







void render(World w) {
  background(235);
  
  // Display all the boxes
  for (int i = 0; i < boxes.size(); i++) {
    Box p = (Box) boxes.get(i);
    p.display();
  }
  
  //smooth();
 /*
  //go through each of our body lists in the physics world
  for (Body body = world.getBodyList(); body != null; body = body.getNext()) 
  { 
   // body.getUserData().display();
    
   
    //Define the shape as a jbox2D shape variable
    org.jbox2d.collision.Shape shape;

    //go through each of our shapes contained in the current body
    for (shape = body.getShapeList(); shape != null; shape = shape.getNext()) { 

      //draw the shapes based on type
      if (shape.getType() == ShapeType.POLYGON_SHAPE) {

        //Polygon Appearance Parameters
        fill(0);



        //Draw the polygon
        polyDraw(body, shape);

      } 
      else if (shape.getType() == ShapeType.CIRCLE_SHAPE) {

        //Circle Appearance Parameters
        fill(0);

        //Draw the circle
        circleDraw(body, shape);
      }
    } 
   
  } */
}

void polyDraw(Body body, org.jbox2d.collision.Shape shape) {

  beginShape();

  PolygonShape poly = (PolygonShape) shape;

  //get the number of vertex points that make up the shape
  int count = poly.getVertexCount();

  //convert the polygon into points
  Vec2[] verts = poly.getVertices();

  //make a vertex for each point of the polygon and convert for pixel coordinates
  for(int i = 0; i < count; i++) {

    //get the position of the vertex of the shape within the body in the world (whew!!)
    Vec2 vert = physics.worldToScreen(body.getWorldPoint(verts[i]));
    vertex(vert.x, vert.y);
  }

  //connect the last point with the first point and stop
  Vec2 firstVert = physics.worldToScreen(body.getWorldPoint(verts[0])); 
  vertex(firstVert.x, firstVert.y);
  endShape();
}

void circleDraw(Body body, org.jbox2d.collision.Shape shape) {

  //convert the shape to a circleshape
  CircleShape circle = (CircleShape) shape;

  //get position of circle within body
  Vec2 center = circle.getLocalPosition();

  //get position of body within world and convert to pixel coordinates
  Vec2 wpoint = physics.worldToScreen(body.getWorldPoint(center));

  //get the radius of the circleshape in pixel format
  float radius = physics.worldToScreen(circle.getRadius());

  //draw the circle with radius converted to diameter
  ellipse(wpoint.x,wpoint.y,radius*2,radius*2);
}
